/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Timer;

/**
 *
 * @author Mentors
 */
public class Manipulator {

    private Elevator elevator;
    private Wrist wrist;
    private Gripper gripper;

    private static DoubleSolenoid minibotSolenoid;

    private Timer timer;

    private int state;

    private final int kIdle           = 0;
    private final int kReleasing      = 1;
    private final int kMovingWrist    = 2;
    private final int kMovingElevator = 3;
    private final int kMovingToFloor  = 4;
    private final int kExtendMinibotSolenoid = 5;
    private final int kMovingElevatorToReleaseMinibot = 6;
    private final int kReleasingMinibot = 7;
    private final int kRetractMinibotSolenoid = 8;

    private static Manipulator m_instance;

    public static Manipulator getInstance() {
        if (m_instance == null) {
            m_instance = new Manipulator();
        }
        return m_instance;
    }

    private Manipulator() {
        elevator = Elevator.getInstance();
        wrist = Wrist.getInstance();
        gripper = Gripper.getInstance();

        minibotSolenoid = new DoubleSolenoid(LogomotionRobot.kDeployMiniForward,
                                             LogomotionRobot.kDeployMiniReverse);

        minibotSolenoid.set(DoubleSolenoid.Value.kReverse);  // this may need to be kForward

        timer = new Timer();
        state = kIdle;
    }

    public void moveToFloor(){
        state = kMovingToFloor;
        gripper.release();
        elevator.moveToFloor();
    }

    public void releaseMinibot(){
        System.out.println("Minibot deployment started");
        state = kReleasingMinibot;
        elevator.moveToFloor();
    }

    public void placeOnPeg(){
//        state = kReleasing;
        state = kMovingWrist;
        gripper.release();
        wrist.release();
//        timer.reset();
//        timer.start();
    }

    public void outputStatus(){
        elevator.outputStatus();
        wrist.outputStatus();
    }

    public void recordStatus(){
        elevator.recordStatus();
        wrist.recordStatus();
    }

    public void checkProgress(){

        elevator.monitorMovement();
        wrist.monitorMovement();

        switch(state){
            case kIdle:
                break;

            case kReleasing:
                if(timer.get() > gripper.getDelay()){
                    state = kMovingWrist;
                    wrist.release();
                }
                break;

            case kMovingWrist:
                if(wrist.onTarget()){
                    if(wrist.fromFloor()){
                        state = kIdle;

                    }else{
                        state = kIdle; // let next script command operate
//                        state = kMovingElevator;
//                        elevator.moveAfterRelease();
                    }
                }
                break;

            case kMovingElevator:
                if(elevator.atPosition()){
                    state = kIdle;
                }
                break;

            case kReleasingMinibot:
                if(elevator.atPosition()){
                    System.out.println("Minibot deployment: Elevator at floor, extending piston " + timer.get());
                    state = kExtendMinibotSolenoid;
                    minibotSolenoid.set(DoubleSolenoid.Value.kForward);
                    timer.reset();
                    timer.start();
                }
                break;

            case kExtendMinibotSolenoid:

                if(timer.get() > 0.25){
                    System.out.println("Minibot deployment: Piston extended, moving to engage carabiner " + timer.get());
                    elevator.moveToCarabinerEngagement();
                    state = kMovingElevatorToReleaseMinibot;
                }
                break;

            case kMovingElevatorToReleaseMinibot:

                if(elevator.atPosition()){
                    System.out.println("Minibot deployment: Carabiner engaged, retracting piston " + timer.get());
                    state = kRetractMinibotSolenoid;
                    minibotSolenoid.set(DoubleSolenoid.Value.kReverse);
                    timer.reset();
                    timer.start();
                }
                break;

            case kRetractMinibotSolenoid:

                if(timer.get() > 0.75){
                    System.out.println("Minibot deployment: Piston retracted, moving elevator up " + timer.get());
                    elevator.moveToMinibotReleasePoint();
                    state = kMovingElevator;
                }
                break;

            case kMovingToFloor:
                if(elevator.atPosition()){
                    state = kIdle;
                    wrist.pickup();
                }
                break;
        }
    }

    public boolean isFinished(){
        return (state == kIdle);
    }
}
